// VirtualJoystick.cpp : implementation file
//

#include "stdafx.h"
#include "NXTPocketPC.h"
#include "VirtualJoystick.h"

#include <math.h>


// CVirtualJoystick

IMPLEMENT_DYNAMIC(CVirtualJoystick, CWnd)

CVirtualJoystick::CVirtualJoystick()
{
	RegisterWindowClass();

	m_JoystickPosition = CPoint(-10,-10);
	m_iArcSize = 20;
	m_iBorderSize = 20;

	m_bJPosOutOfRadius = false;

	m_pNxtBrick = NULL;
}

CVirtualJoystick::~CVirtualJoystick()
{
}


BEGIN_MESSAGE_MAP(CVirtualJoystick, CWnd)
	ON_WM_ERASEBKGND()
	ON_WM_PAINT()
	ON_WM_LBUTTONDOWN()
	ON_WM_LBUTTONUP()
	ON_WM_MOUSEMOVE()
END_MESSAGE_MAP()

void CVirtualJoystick::UpdateJoystick(CDC* pDC)
{

	CRect rectClient;
	CBrush brushBack,brushWhite,brushJoystick;
	CBrush* pOldBrush;
	CPen penJoystick,penBack;
	CPen* pOldPen;

	brushWhite.CreateSolidBrush(RGB(255,255,255));
	brushBack.CreateSolidBrush(RGB(209,209,209));
	brushJoystick.CreateSolidBrush(RGB(193,13,86));

	penBack.CreatePen(PS_SOLID , 1, RGB(209,209,209));
	penJoystick.CreatePen(PS_SOLID , 1, RGB(193,13,86));

	GetClientRect(rectClient);

	//clear Background
	//pDC->FillRect(rectClient,&brushWhite);

	pOldPen = pDC->SelectObject(&penBack);
	pOldBrush = pDC->SelectObject(&brushBack);

	pDC->Ellipse(rectClient.top+m_iBorderSize,rectClient.left+m_iBorderSize,
				rectClient.right-m_iBorderSize,rectClient.bottom-m_iBorderSize);

	pDC->SelectObject(&penJoystick);
	pDC->SelectObject(&brushJoystick);

	pDC->Ellipse(m_JoystickPosition.x - m_iArcSize, m_JoystickPosition.y - m_iArcSize,
				m_JoystickPosition.x + m_iArcSize, m_JoystickPosition.y + m_iArcSize);

	//restore old pen.
	pDC->SelectObject(pOldPen);
	pDC->SelectObject(pOldBrush);
}

void CVirtualJoystick::CenterJoystick()
{
	CRect rectClient;
	GetClientRect(rectClient);

	//center means stop motors
	m_pNxtBrick->Stop();

	m_JoystickPosition =  rectClient.CenterPoint();

	CClientDC dc(this); 

	UpdateJoystick(&dc);
}

BOOL CVirtualJoystick::OnEraseBkgnd(CDC* pDC)
{
	CRect rectClient;
	CBrush brushWhite;

	brushWhite.CreateSolidBrush(RGB(255,255,255));

	//clear Background
	GetClientRect(rectClient);
	pDC->FillRect(rectClient,&brushWhite);

	return CWnd::OnEraseBkgnd(pDC);
}

void CVirtualJoystick::OnPaint()
{
	CPaintDC dc(this); 

	CRect rectClient;
	CBrush brushWhite;

	brushWhite.CreateSolidBrush(RGB(255,255,255));

	//clear Background
	GetClientRect(rectClient);
	dc.FillRect(rectClient,&brushWhite);

	UpdateJoystick(&dc);
}

void CVirtualJoystick::OnLButtonDown(UINT nFlags, CPoint point)
{
	TRACE(L"LButton down (x=%d / y=%d)\n",point.x,point.y);

	CClientDC dc(this);

	SetMotorsSpeed(point);

	m_JoystickPosition = point;

	if(!m_bJPosOutOfRadius)  UpdateJoystick(&dc);
}

void CVirtualJoystick::OnLButtonUp(UINT nFlags, CPoint point)
{
	TRACE(L"LButton up (x=%d / y=%d)\n",point.x,point.y);
	CenterJoystick();
}

void CVirtualJoystick::OnMouseMove(UINT nFlags, CPoint point)
{
//	TRACE(L"mouse move (x=%d / y=%d)\n",point.x,point.y);

	CClientDC dc(this);

	SetMotorsSpeed(point);

	m_JoystickPosition = point;

	if(!m_bJPosOutOfRadius)  UpdateJoystick(&dc);

}
void CVirtualJoystick::SetMotorsSpeed(CPoint &point)
{
	double fx1,fy1,fx2,fy2;
	double ang,d,f;
	const double PI = 3.141592653589;
	int x,y,x1,y1,r,i;

	CRect rectClient;
	GetClientRect(rectClient);

	CPoint ptCenter = rectClient.CenterPoint();
	CSize szDiff = ptCenter - point;

//	TRACE(L"diff = x=%d,y=%d ",szDiff.cx,szDiff.cy,ptCenter.x,ptCenter.y);

	//calc to percent
	szDiff.cx = (int) ( (100.0 / (ptCenter.x-m_iBorderSize)) * szDiff.cx );  
	szDiff.cy = (int) ( (100.0 / (ptCenter.y-m_iBorderSize)) * szDiff.cy  );  

	//calc radius
	r = (int) sqrt((szDiff.cx*szDiff.cx)+(szDiff.cy*szDiff.cy));

//	TRACE(L"diff(%%) = x=%d,y=%d (center=%d/%d)(r=%d)\n",szDiff.cx,szDiff.cy,ptCenter.x,ptCenter.y,r);


	//do some coordinate transformation
	//=================================

	//first a reflection on 45deg
	ang = (PI * 45)/180.0;
	fx1 = (szDiff.cx * cos(2*ang)) + ( szDiff.cy * sin(2*ang));
	fy1 = (szDiff.cx * sin(2*ang)) + (-1 * szDiff.cy * cos(2*ang));

	//second a 45deg  around z
	ang = (PI * 45)/180.0;
	fx2 = (fx1 * cos(ang)) + (-1 *  fy1 * sin(ang));
	fy2 = (fx1 * sin(ang)) + (fy1 * cos(ang));

	//normalizing r to 1-10
	r = (int)((r * 1.25) / 10);

	// do not exceed 10
	if (r>10) r = 10;

	m_bJPosOutOfRadius = r == 10;

	x = (int) fx2;
	y = (int) fy2;

	//normalizing x and y, 1 to r
	if(fx2==0 && fy2==0)
	{
		d = 0;
	}
	else if(fabs(fx2)>fabs(fy2))
	{
		d = r / fabs(fx2);
	}
	else
	{
		d = r / fabs(fy2);
	}		

	x1 = (int) (fx2 * d);
	y1 = (int) (fy2 * d);
	

	if(r<2)//on very small radius
	{
		//go slow forward
		x1=y1=1;
	}	
	else if(r<4) //level motors value on small radius
	{
		if(x1>y1) y1=x1;
		if(y1>x1) x1=y1;
	}

	//if motion is close together, level it
	//first calc absolute diff
	i = abs(abs(x1) - abs(y1));
	if( i < 4 )
	{
		if(abs(x1)>abs(y1))
		{
			if(y1<0)
				y1 -= i;
			else
				y1 += i;
		}
		else
		{
			if(x1<0)
				x1 -= i;
			else
				x1 += i;

		}
	}

	TRACE(L"points: x=%3d,y=%3d  | x1=%3d,y1=%3d  fx2=%6.2f,fy2=%6.2f (r=%2d/d=%.2f/i=%d)\n",x,y,x1,y1,fx2,fy2,r,d,i);

	m_pNxtBrick->Drive(x1*10,y1*10);

/*
	//calc annulus 
	i = (r/3) + 1;
	if(i<2) i=2;

	//normalizing x and y, 1 to i
	if(fx2==0 && fy2==0)
	{
		d = 0;
	}
	else if(fabs(fx2)>fabs(fy2))
	{
		d = i / fabs(fx2);
	}
	else
	{
		d = i / fabs(fy2);
	}

	x1 = (int) (x * d);
	y1 = (int) (y * d);

	//special case in center: does not allow stop a motor
	if(r<3 && (x1==0 || y1==0))
	{
		if(x1==0) x1 = y1;
		if(y1==0) y1 = x1;
	}

	//normaliz x and y to r
	if(x1==0 && y1==0)
	{
		d = 0;
	}
	else if(abs(x1)>abs(y1))
	{
		d = r / fabs(x1);
	}
	else
	{
		d = r / fabs(y1);
	}

	x = (int) (x1 * d);
	y = (int) (y1 * d);
	

	TRACE(L"points: x=%2d,y=%2d  | x1=%2d,y1=%2d  fx2=%2.2f,fy2=%2.2f (r=%2d/d=%.2f/i=%d)\n",x,y,x1,y1,fx2,fy2,r,d,i);
	*/
}

bool CVirtualJoystick::RegisterWindowClass(void)
{
    WNDCLASS wndcls;
    HINSTANCE hInst = AfxGetResourceHandle();

    if (!(::GetClassInfo(hInst, VIRTUALJOYSTICK_CLASSNAME, &wndcls)))
    {
        // otherwise we need to register a new class

        wndcls.style            = CS_DBLCLKS | CS_HREDRAW | CS_VREDRAW;
        wndcls.lpfnWndProc      = ::DefWindowProc;
        wndcls.cbClsExtra       = wndcls.cbWndExtra = 0;
        wndcls.hInstance        = hInst;
        wndcls.hIcon            = NULL;
        wndcls.hCursor          = AfxGetApp()->LoadStandardCursor(IDC_ARROW);
        wndcls.hbrBackground    = (HBRUSH) (COLOR_3DFACE + 1);
        wndcls.lpszMenuName     = NULL;
        wndcls.lpszClassName    = VIRTUALJOYSTICK_CLASSNAME;

        if (!AfxRegisterClass(&wndcls))
        {
            AfxThrowResourceException();
            return false;
        }
    }

    return true;
}

BOOL CVirtualJoystick::Create(CWnd* pParentWnd, const RECT& rect, UINT nID, DWORD dwStyle /*=WS_VISIBLE*/)
{
	ASSERT(m_pNxtBrick);

	return CWnd::Create(VIRTUALJOYSTICK_CLASSNAME, _T(""), dwStyle, rect, pParentWnd, nID);
}



